package test;

import java.awt.BasicStroke;
import java.awt.Color;
import java.awt.Dimension;
import java.awt.Graphics;
import java.awt.Graphics2D;
import java.awt.RenderingHints;
import java.awt.geom.AffineTransform;
import java.awt.geom.Line2D;
import java.awt.geom.Path2D;
import java.util.Vector;

import javax.swing.JComponent;
import javax.swing.JFrame;

import org.ejml.simple.SimpleMatrix;

public class kalman_filter {

	private SimpleMatrix A;
	private SimpleMatrix B;
	private SimpleMatrix H;
	private SimpleMatrix current_state_estimate;
	private SimpleMatrix current_prob_estimate;
	private SimpleMatrix Q;
	private SimpleMatrix R;

	public kalman_filter(SimpleMatrix _A, SimpleMatrix _B, SimpleMatrix _H, SimpleMatrix _x, SimpleMatrix _P, SimpleMatrix _Q, SimpleMatrix _R){
		this.A = _A;
		this.B = _B;
		this.H = _H;
		this.current_state_estimate = _x;
		this.current_prob_estimate = _P;
		this.Q = _Q;
		this.R = _R;
	}
	
	public SimpleMatrix getCurrentState(){
		return this.current_state_estimate;
	}
	public void Step(SimpleMatrix control_vector, SimpleMatrix measurement_vector) {
		/* Prediction step */
		SimpleMatrix predicted_state_estimate = this.A.mult(this.current_state_estimate).plus(this.B.mult(control_vector));
		SimpleMatrix predicted_prob_estimate = this.A.mult(this.current_prob_estimate).mult(this.A.transpose()).plus(this.Q);
		/* Observation step */
		SimpleMatrix innovation = this.H.mult(measurement_vector).minus(predicted_state_estimate);
		SimpleMatrix innovation_covariance = this.H.mult(predicted_prob_estimate).mult(this.H.transpose()).plus(this.R);
		/* Update step */
		SimpleMatrix kalman_gain = predicted_prob_estimate.mult(this.H.transpose()).mult(innovation_covariance.invert());
		this.current_state_estimate = predicted_state_estimate.plus(kalman_gain.mult(innovation));
		int size = this.current_prob_estimate.numRows();
		this.current_prob_estimate = (SimpleMatrix.identity(size).minus(kalman_gain.mult(this.H))).mult(predicted_prob_estimate);
	}
	
	
	
	static class Cannon{
		int angle = 45;
		int muzzle_velocity = 100;
		double[] gravity = {0,-9.81};
		double[] velocity = { muzzle_velocity*Math.cos(angle*Math.PI/180.0) , muzzle_velocity*Math.sin(angle*Math.PI/180.0) };
		double[] loc = {0,0};
		double[] acceleration = {0,0};
		private double timeslice;
		private double noiselevel;
		
		public Cannon(double _timeslice, double _noiselevel){
			this.timeslice = _timeslice;
			this.noiselevel = _noiselevel;
		}
		
		double getX(){return loc[0];}
		double getY(){return loc[1];}
		double getXwithNoise(){return getX()+(Math.random()-0.5)*20;}
		double getYwithNoise(){return getY()+(Math.random()-0.5)*20;}
		double getXvelocity(){return this.velocity[0];}
		double getYvelocity(){return this.velocity[1];}
		
		public void step(){
			double[] timeslicevec = {this.timeslice,this.timeslice};
			double[] sliced_gravity = {this.gravity[0]*timeslicevec[0] , this.gravity[1]*timeslicevec[1]};
			double[] sliced_acceleration = sliced_gravity;
			this.velocity = new double[] { this.velocity[0]+sliced_acceleration[0] , this.velocity[1]+sliced_acceleration[1]};
			double [] sliced_velocity = new double[] {this.velocity[0]*timeslicevec[0] , this.velocity[1]*timeslicevec[1]};
			this.loc = new double[] {this.loc[0]+sliced_velocity[0] , this.loc[1]+sliced_velocity[1]};
			if (this.loc[1]<0) this.loc[1]=0;
		}
	}
	
	
	public static void main(String[] args) {
		double timeslice = 0.1;
		int iterations = 144;
		double noiselevel = 30;
		int muzzle_velocity = 100;
		int angle = 45;
		
		final Vector<Double> x = new Vector<Double>();
		final Vector<Double> y = new Vector<Double>();
		final Vector<Double> nx = new Vector<Double>();
		final Vector<Double> ny = new Vector<Double>();
		final Vector<Double> kx = new Vector<Double>();
		final Vector<Double> ky = new Vector<Double>();
		
		Cannon c = new Cannon(timeslice,noiselevel);
		
		double speedX = muzzle_velocity*Math.cos(angle*Math.PI/180.0);
		double speedY = muzzle_velocity*Math.sin(angle*Math.PI/180.0);
		
		SimpleMatrix state_transition = new SimpleMatrix(new double[][]{{1,timeslice,0,0},{0,1,0,0},{0,0,1,timeslice},{0,0,0,1}});
		SimpleMatrix control_matrix = new SimpleMatrix(new double[][]{{0,0,0,0},{0,0,0,0},{0,0,1,0},{0,0,0,1}});
		SimpleMatrix control_vector = new SimpleMatrix(new double[][]{{0},{0},{-9.81*timeslice*timeslice},{-9.81*timeslice}});
		
		SimpleMatrix observation_matrix = SimpleMatrix.identity(4);
		
		SimpleMatrix initial_state = new SimpleMatrix(new double[][]{{0},{speedX},{500},{speedY}}); // y=500 damit man sieht wie toll der kalmanfilter is
		
		SimpleMatrix initial_probability = SimpleMatrix.identity(4);
		
		SimpleMatrix process_covariance = SimpleMatrix.identity(4);
		
		SimpleMatrix measurement_covariance = (SimpleMatrix.identity(4)).scale(1);
		
		kalman_filter kf = new kalman_filter(state_transition, control_matrix, observation_matrix, initial_state, initial_probability, process_covariance, measurement_covariance);
		
		for (int i = 0; i<iterations;i++){
			x.add(c.getX());
			y.add(c.getY());
			double newestX = c.getXwithNoise();
			double newestY = c.getYwithNoise();
			nx.add(newestX);
			ny.add(newestY);
			c.step();
			kx.add(kf.getCurrentState().get(0,0));
			ky.add(kf.getCurrentState().get(2,0));
			kf.Step(control_vector, new SimpleMatrix(new double[][]{{newestX},{c.getXvelocity()},{newestY},{c.getYvelocity()}}));
			
		}
		
		/* ANZEIGE */
		JFrame frame = new JFrame();
		
		// get maximumse
		final double[] extX = getminmax(x,nx,kx);
		final double[] extY = getminmax(y,ny,ky);
		
		System.out.println("extX == "+extX[0]+" : "+extX[1]);
		System.out.println("extY == "+extY[0]+" : "+extY[1]);
		
		
//		frame.setBounds(100,100,500,300);
		frame.setDefaultCloseOperation(JFrame.EXIT_ON_CLOSE);
		
//		final AffineTransform scale = AffineTransform.getScaleInstance(1000, 600);
		final AffineTransform scale = AffineTransform.getScaleInstance(950, 550);
		scale.preConcatenate(AffineTransform.getTranslateInstance(25, 25));
		
		JComponent comp = new JComponent() {
			{setPreferredSize(new Dimension(1000,600));}
			
			
			Path2D p = new Path2D.Double();
			Line2D l = new Line2D.Double();
			double step = 0.1;
			double inset = 0.02;
			
			@Override
			public void paint(Graphics arg0) {
				super.paint(arg0);
				double deltaX = extX[1]-extX[0];
				double deltaY = extY[1]-extY[0];
				
				Graphics2D g2 = (Graphics2D) arg0;
				g2.setRenderingHint(RenderingHints.KEY_ANTIALIASING,
						RenderingHints.VALUE_ANTIALIAS_ON);
				g2.setRenderingHint(RenderingHints.KEY_STROKE_CONTROL,
						RenderingHints.VALUE_STROKE_PURE);
				g2.setRenderingHint(RenderingHints.KEY_RENDERING,
						RenderingHints.VALUE_RENDER_QUALITY);
				g2.setStroke(new BasicStroke(2f));
				
				
				// kanone
				p.reset();
				p.moveTo(0, 1);
				for (int i=0;i<x.size();i++){
					p.lineTo( (x.get(i)-extX[0])/deltaX, 1-((y.get(i)-extY[0])/deltaY) ) ;
				}
				
				g2.setColor(Color.blue);
				g2.draw(scale.createTransformedShape(p));
			
				p.reset();
				p.moveTo(0, 1);
				for (int i=0;i<x.size();i++){
					p.lineTo( (nx.get(i)-extX[0])/deltaX, 1-((ny.get(i)-extY[0])/deltaY) ) ;
				}
				g2.setColor(Color.red);
				g2.draw(scale.createTransformedShape(p));

				p.reset();
				p.moveTo(0, 1);
				for (int i=0;i<x.size();i++){
					p.lineTo( (kx.get(i)-extX[0])/deltaX, 1-((ky.get(i)-extY[0])/deltaY) ) ;
				}
				g2.setColor(Color.green);
				g2.draw(scale.createTransformedShape(p));
				
				g2.setStroke(new BasicStroke(1f));
				// achsen
				p.reset();
				p.moveTo(0, 0);
				p.lineTo(0, 1);
				p.lineTo(1, 1);
				g2.setColor(Color.black);
				g2.draw(scale.createTransformedShape(p));
				
				for (double d = 0; d<1; d+=step) {
					l.setLine(-inset, d, inset, d);
					g2.draw(scale.createTransformedShape(l));
					l.setLine(d, 1-inset, d, 1+inset);
					g2.draw(scale.createTransformedShape(l));
				}

			}
		};
		
		frame.add(comp);
		frame.pack();
		frame.setVisible(true);
		
		
	}
	static double[] getminmax(Vector<Double> x, Vector<Double> nx, Vector<Double> kx){
		double min=0,max=0, tmp=0;
		for (int i=0;i<x.size();i++){
			tmp = x.get(i);
			if (tmp<min) min=tmp;
			if (tmp>max) max=tmp;
		}
		tmp = 0;
		for (int i=0;i<nx.size();i++){
			tmp = nx.get(i);
			if (tmp<min) min=tmp;
			if (tmp>max) max=tmp;
		}
		tmp = 0;
		for (int i=0;i<kx.size();i++){
			tmp = kx.get(i);
			if (tmp<min) min=tmp;
			if (tmp>max) max=tmp;
		}
		return new double[]{min,max};
	}
}

